#include <ctype.h>
#include <stdarg.h>

void init_mutils()
{
  terminate_timer = 0;
  global_status = GS_IDLE;
  
#ifdef PERFORM_STACKTEST
  basestack_ptr = NULL; max_stackusage = 0;
#endif

#ifdef USE_BLUSH
#ifdef SAVE_DEBUG
  memset(save_debug, 0, sizeof(save_debug));
  savedebug_begin = 0; savedebug_end = 0;
#endif
#endif
}

#ifdef PERFORM_STACKTEST
void do_stacktest(int stack_usage, const char *fn)
{
  if (stack_usage > max_stackusage)
  {
    max_stackusage = stack_usage;
    DBGOUT("Stack usage: %d, Max stack usage: %d, function: %s", stack_usage, max_stackusage, fn);
  }
}
#endif

void format_debugstr()
{
  char *p;

  for (;;)
  {
    p = strstr(debug_str, "$");
    if (p == NULL)
      break;

    strcpy(debug_str, p+1);
  }
}

void dbgout(char *fmt, ...)
{
#ifdef TOSSIM
  va_list argptr;
  char cmd[512];
  FILE *f;

  va_start(argptr, fmt);
  vsnprintf(cmd, sizeof(cmd), fmt, argptr);
  va_end(argptr);

  dbg("dbg_usr1", "%s%s\n", debug_str, cmd);
  if (debug_str[0])
  {
    f = fopen(ERRORLOG_FILE, "a");
    fprintf(f, "DEBUG (%d): %s%s\n", TOS_NODE_ID, debug_str, cmd);
    fclose(f);
  }
#else
#ifdef USE_BLUSH
  va_list argptr;
  char cmd[512];
#ifdef SAVE_DEBUG  
  char sd1[512];
#endif  

  va_start(argptr, fmt);
  vsnprintf(cmd, sizeof(cmd), fmt, argptr);
  va_end(argptr);  
  
  if (blush_debugoutput)
    trace(DBG_USR1, "DEBUG (%d): %s%s\n", TOS_NODE_ID, debug_str, cmd);
#ifdef SAVE_DEBUG 
  else {
    if (save_debug[savedebug_end])
    {
      free(save_debug[savedebug_end]);
      savedebug_begin = (savedebug_begin+1) % MAX_DEBUGSTR;
    }
  
    snprintf(sd1, sizeof(sd1), "DEBUG (%d,%u): %s%s", TOS_NODE_ID, timer_cycles, debug_str, cmd);      
    save_debug[savedebug_end] = (char *) malloc(strlen(sd1)+1);
    strcpy(save_debug[savedebug_end], sd1);    
    savedebug_end = (savedebug_end+1) % MAX_DEBUGSTR;
  }
#endif 
 
#endif
#endif
}

char *strupr(char *str)
{
  char *string = str;

  if (str)
    for ( ; *str; ++str)
      *str = toupper(*str);
  return string;
}

void reverse_pathvec(ADDRESS *path, uint8_t len)
{
  ADDRESS t;
  int i;

  for (i = 0; i < len/2; i++)
  {
    t = path[i];
    path[i] = path[len-i-1];
    path[len-i-1] = t;
  }
}

int find_pathpos(uint8_t path_len, ADDRESS *path, ADDRESS node) // guess which is the position of "node" on the path vector
{
  int i;

  for (i = 0; i < path_len; i++)
    if (path[i] == node)
      return i;

  return -1;
}

void share_pathprefix(uint8_t path_len1, ADDRESS *path1, uint8_t *path_len2, ADDRESS **path2)
{
  int i, j, k, consec_check = (ADDRDOMAIN_BITS <= ADDR_BITS) ? 1 : 3;
  uint8_t consecutive;
  ADDRESS path[MAX_PATHLEN];

DO_STACKTEST
 
  for (i = path_len1-1; i >= consec_check-1; i--)
    for (j = *path_len2-1; j >= i; j--)
      if (path1[i] == (*path2)[j])
      {
        consecutive = 1;
        for (k = 0; k < consec_check; k++)
          if (path1[i-k] != (*path2)[j-k])
          {
            consecutive = 0;
            break;
          }
        if (consecutive == 0)
          break;

        memcpy(path, path1, i*sizeof(ADDRESS));
        memcpy(&path[i], &(*path2)[j], (*path_len2-j)*sizeof(ADDRESS));

        if (i != j || memcmp(path, *path2, *path_len2*sizeof(ADDRESS)))
        {
#ifdef TOSSIM
          print_path(*path2, *path_len2, "Old prefix path");
#endif
          mfree(*path2);
          (*path_len2) += i-j;
          *path2 = (ADDRESS *) mcalloc(*path_len2, sizeof(ADDRESS));
          memcpy(*path2, path, *path_len2*sizeof(ADDRESS));
#ifdef TOSSIM
          print_path(*path2, *path_len2, "Updated prefix path");
#endif
          return;
        }
      }
}

void pause_sim()
{
#ifdef TOSSIM
  char c;
  switch (config.pause)
  {
    case 0:
      DBGOUT("Continue? (y/n)");
      scanf("%c", &c);
      if (c == 'n')
        exit(1);
      break;
    case 1: exit(1);
    default: return;
  }
#endif

  set_globalstatus(GS_ERROR);
}

void print_hexlist(char *s, uint8_t *l, int size)
{
  int i, len;

  s[0] = 0;
  for (i = 0; i < size; i++)
  {
    len = strlen(s);
    sprintf(&s[len], "%02X ", l[i]);
  }
}

void print_grouplist(uint8_t *group, int size)
{
  char s[256];

  print_hexlist(s, group, size);
  DBGOUT("Sent sequences: %s", s);
}

void print_list(uint8_t *n, int count, char *l)
{
  int i, len;
  char s[1024];
  memset(s, 0, sizeof(s));

  for (i = 0; i < count; i++)
  {
    len = strlen(s);
    sprintf(&s[len], "%d ", n[i]);
  }
  DBGOUT("%s: %s", l, s);
}

void print_path(ADDRESS *n, int count, char *l)
{
  int i, len;
  char s[256];
  memset(s, 0, sizeof(s));

  for (i = 0; i < count; i++)
  {
    len = strlen(s);
    sprintf(&s[len], "%d ", n[i]);
  }
  DBGOUT("%s: %s", l, s);
}

void print_tuple(ATTRIBUTE *n, int count, char *l)
{
  int i, len;
  char s[256];
  memset(s, 0, sizeof(s));

  for (i = 0; i < count; i++)
  {
    len = strlen(s);
    sprintf(&s[len], "%d ", n[i]);
  }
  DBGOUT("%s: %s", l, s);
}

void print_sel(uint8_t *sel)
{
  DBGOUT("Selectivities: (%d, %d, %d)", sel[REL_S], sel[REL_T], sel[REL_J]);
}

#if defined (TOSSIM) || defined (UNIT_TEST)
void doprint_stack(STACK *s, char *str, int parent_priority)
{
  int i, len;
  TOKEN t;

  stack_pop(s, &t);
  switch (t.type)
  {
    case TOKEN_OP:
      if (ops[OPGET_TYPE(t.value)].arg_count+OPGET_EXTRAARG(t.value) > 1)
      {
        if (ops[OPGET_TYPE(t.value)].priority < parent_priority)
        {
          len = strlen(str);
          sprintf(&str[len], "(");
        }

        doprint_stack(s, str, ops[OPGET_TYPE(t.value)].priority);

        len = strlen(str);
        sprintf(&str[len], " %s ", (ops[OPGET_TYPE(t.value)].id == OP_MOD) ? "%%" : ops[OPGET_TYPE(t.value)].name);

        for (i = 1; i < ops[OPGET_TYPE(t.value)].arg_count+OPGET_EXTRAARG(t.value); i++)
        {
          if (i > 1)
          {
            len = strlen(str);
            sprintf(&str[len], " ");
          }
          doprint_stack(s, str, ops[OPGET_TYPE(t.value)].priority);
        }

        if (ops[OPGET_TYPE(t.value)].priority < parent_priority)
        {
          len = strlen(str);
          sprintf(&str[len], ")");
        }
      } else {
        len = strlen(str);
        sprintf(&str[len], "%s(", ops[OPGET_TYPE(t.value)].name);

        for (i = 0; i < ops[OPGET_TYPE(t.value)].arg_count+OPGET_EXTRAARG(t.value); i++)
          doprint_stack(s, str, ops[OPGET_TYPE(t.value)].priority);

        len = strlen(str);
        sprintf(&str[len], ")");
      }
      break;

    case TOKEN_NUM:
      len = strlen(str);
      sprintf(&str[len], "%d", t.value);
      break;

    case TOKEN_ATTR:
      len = strlen(str);
      if (ATTRGET_TEMP(t.value) == 0)
        sprintf(&str[len], "%s.%s%s", rel_name[ATTRGET_REL(t.value)], attr_name[ATTRGET_TYPE(t.value)],
          ATTRGET_SEQ(t.value) ? "#" : "");
      else sprintf(&str[len], "%s.temp%d", rel_name[ATTRGET_REL(t.value)], ATTRGET_TYPE(t.value));
      break;

    default:
      DBGERR("Invalid token type: %d", t.type);
      pause_sim();
      return;                                                                
  }
}

void print_stack(void *s1)
{
  STACK t, *s = (STACK *) s1;
  char str[1024] = "";

  if (s->stack_len == 0)
  {
    DBGOUT("(empty stack)");
    return;
  }

  memcpy(&t, s, sizeof(STACK));
  doprint_stack(&t, str, -100);

  DBGOUT(str);
}
#endif

#ifdef TOSSIM
FILE *file_open(char *name, char *mask)
{
  FILE *f = fopen(name, mask);

  if (f == NULL)
  {
    DBGERR("Cannot open file %s with mask %s", name, mask);
    exit(1);
  }

  return f;
}

uint8_t check_nodefailed(ADDRESS node_id)
{
  int i;

  for (i = 0; i < config.fail_count; i++)
    if (config.fail_nodes[i].node == node_id && timer_cycles+10 > config.fail_nodes[i].time)
      return 1;

  return 0;
}

int check_routeterminate(uint8_t busy_flag, uint16_t node_count)
{
  char s[256];
  FILE *f;
  int i, k;

  if (timer_cycles % 20 == 9)
  {
    snprintf(s, sizeof(s), NODEBUSY_FILE, TOS_NODE_ID);
    f = fopen(s, "w");
    if (f != NULL)
    {
      fprintf(f, "%d", busy_flag);
      fclose(f);
    }
  }

  if (TOS_NODE_ID == 0 && busy_flag == 0 && timer_cycles % 20 == 19)
  {
    for (i = 0; i < node_count; i++)
      if (check_nodefailed(i) == 0)
      {
        snprintf(s, sizeof(s), NODEBUSY_FILE, i);
        f = fopen(s, "r");
        if (f == NULL)
        {
          terminate_timer = 0;
          return 0;
        }

        fscanf(f, "%d", &k);
        if (k != 0)
        {
          terminate_timer = 0;
          return 0;
        }
        fclose(f);
      }

    DBGOUT("Looks like all nodes have terminated...");
    terminate_timer++;
    if (terminate_timer >= 2)
    {
      DBGOUT("All nodes terminated.");
      return 1;
    }
  }

  return 0;
}
#endif

ADDRESS *alloc_addr(ADDRESS addr)
{
  ADDRESS *p = (ADDRESS *) mmalloc(sizeof(ADDRESS));
  *p = addr;
  return p;
}

uint16_t compute_crc(ADDRESS addr, uint8_t *data, uint8_t len)
{                         // compute checksum of address+payload
  int i;
  uint16_t crc = addr;

  for (i = 0; i < len; i++)
    crc = 1127*crc+data[i];

  return crc;
}

int encode_bitdiff(uint8_t *buffer1, uint8_t *buffer2, uint8_t len, uint8_t *diff, uint8_t max_diff)
{
  int i, diff_count = 0;

  for (i = 0; i < len*8; i++)
    if (BV_GET(buffer1, i) != BV_GET(buffer2, i))
    {
      diff[diff_count++] = i;
      if (diff_count > max_diff)
        return -1;
    }

  return diff_count;      
}

int decode_bitdiff(uint8_t *buffer1, uint8_t *buffer2, uint8_t len, uint8_t *diff, uint8_t diff_len)
{
  int i;

  memcpy(buffer2, buffer1, len);
  for (i = 0; i < diff_len; i++)
    BV_SET(buffer2, diff[i], 1-BV_GET(buffer1, diff[i]));

  return 0;
}

int32_t int_sqrt(int32_t n)
{
  int32_t x = n, x1;

  if (n <= 0)
    return 0;

  for (;;)
  {
    x1 = (x+n/x) >> 1;
    if (abs(x-x1) <= 1)
      return x1;
    x = x1;
  }
}


#define MAX_WINDOWOFS   255
#define MAX_WINDOWLEN   127

// LZ77 ASCII compression/decompression (character code < 128). Assumes highest bit of input characters is always 0. Offset: 8 bits. Length: 7 bits
void compressASCII(uint8_t *buf, int16_t buf_len, uint8_t *out, int16_t *out_len)
{
  int16_t buf_pos = 0, match_pos, match_len, bestmatch_pos, bestmatch_len;

  *out_len = 0;  
  while (buf_pos < buf_len)
  {
    bestmatch_pos = 0; bestmatch_len = 0;
    match_pos = MAX(0, buf_pos-MAX_WINDOWOFS);

    while (match_pos < buf_pos)
    {
      for (match_len = 0; match_len < MAX_WINDOWLEN &&
        buf[match_pos+match_len] == buf[buf_pos+match_len] && buf_pos+match_len < buf_len; match_len++)
        if (match_len > 1 && match_len > bestmatch_len)
        {
          bestmatch_len = match_len;
          bestmatch_pos = match_pos;
        }

      match_pos += MAX(match_len, 1);  
    }
    
    if (bestmatch_len > 0)
    {
      out[(*out_len)++] = (0x80 | bestmatch_len);
      out[(*out_len)++] = buf_pos-bestmatch_pos;
      buf_pos += (bestmatch_len+1);
    } else out[(*out_len)++] = (buf[buf_pos++] & 0x7f);
  }
}

void decompressASCII(uint8_t *buf, int16_t buf_len, uint8_t *out, int16_t *out_len)
{
  int16_t buf_pos = 0, match_pos, match_len, i;

  *out_len = 0;
  while (buf_pos < buf_len)
    if ((buf[buf_pos] & 0x80) == 0)
      out[(*out_len)++] = buf[buf_pos++];
    else {
      match_len = (buf[buf_pos] & 0x7f);
      match_pos = (*out_len) - buf[buf_pos+1];
      buf_pos += 2;
      for (i = 0; i < match_len+1; i++)
        out[(*out_len)++] = out[match_pos+i];
    }  
}

uint32_t timercycles_to_seconds(uint32_t cycles)
{
  return (TIMER1_INTERVAL < 1000) ? (cycles/(1000/TIMER1_INTERVAL)) : (cycles*(TIMER1_INTERVAL/1000));
}

uint32_t seconds_to_timercycles(uint32_t seconds)
{
  return (TIMER1_INTERVAL < 1000) ? (seconds*(1000/TIMER1_INTERVAL)) : (seconds/(TIMER1_INTERVAL/1000));
}

uint8_t check_congestion()
{
  return (outputbuffer_size > OUTPUTBUFFER_SIZE-CONG_MINOUTPUT ||
    mergebuffer_size > MERGEBUFFER_SIZE-CONG_MINMERGE ||
    payloadbuffer_size > PAYLOADBUFFER_SIZE-CONG_MINPAYLOAD || heapman_available() < CONG_MINHEAP);
}

uint8_t node_idle()
{
  return (outputbuffer_sending == 0 && outputbuffer_size == 0 && mergebuffer_size == 0 && payloadbuffer_size == 0);
}

void set_globalstatus(uint8_t gs)
{
  DBGOUT("Global status: %d", gs);
  global_status = gs;
}

